#include <filesystem>
#include "axisgroup_set.h"
#include "axisgroup/axisgroup.h"
#include "log/zuclog.h"
#include "config.h"
#include "jkutil/iniparser.h"
#include "kine/kine.h"
#include "dyna/jaka_dyn_intf.h"
#include "axis/axis.h"
#include "ag_config.h"
#include "zuc/rob_common/robtool.h"
#include "zuc/rob_common/robot_payload.h"
#include "zuc/rob_common/usrframe.h"

#define AXISGROUP_LOG_INFO(fmt, ...) zuclog_info("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)
#define AXISGROUP_LOG_ERR(fmt, ...) zuclog_error("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)

axisgroup::RobotSetIntf::RobotSetIntf(int id, AxisGroup* grp) : group_id_(id), grp_(grp)
{
    std::string user_cfg = ZUC_USER_CONFIG_DIR + std::string("/robconfig_") + std::to_string(group_id_) + ".ini";

    //若机器人用户配置文件不存在，则拷贝默认配置文件
    std::filesystem::path default_cfg = ZUC_DEFAULT_CONFIG_DIR + std::string("/robconfig.ini");
    try
    {
        std::filesystem::copy_file(default_cfg, user_cfg, std::filesystem::copy_options::skip_existing);
    }
    catch (std::exception& e)
    {
        AXISGROUP_LOG_ERR("{}", e.what());
    }

    //加载机器人序列号
    Util::IniParser usr_parser(user_cfg.c_str());
    usr_parser.get("ROBOT.SERIAL_NUM", serial_num_, 3119999);
}

axisgroup::RobotSetIntf::~RobotSetIntf() {}

int axisgroup::RobotSetIntf::load_config(int serial_num)
{
    auto file = axisgroup::AgConfig::find_model_config(serial_num);
    std::string user_cfg = ZUC_USER_CONFIG_DIR + std::string("/robconfig_") + std::to_string(group_id_) + ".ini";

    //#TODO 写机型号
    serial_num_ = serial_num;
    ag_config_.write_serial_num(user_cfg, serial_num_);

    //优先加载模型配置文件，然后加载用户配置文件，从而保证用户配置文件的优先级更高
    ag_config_.load_model_config(file);
    ag_config_.load_usr_config(user_cfg);
    is_config_change_ = is_config_change_ || ag_config_.fit_to_robot_config();  // 调整部分限制值不超过机型定义

    ag_config_.config_dh(get_joint_num_by_serial_num(serial_num));  // 将DH参数转化为标准形式;

    config_change();
    update_active_config();
    return 0;
}

void axisgroup::RobotSetIntf::config_change()
{
    is_config_change_ = true;
    is_config_synced_ = false;
}

void axisgroup::RobotSetIntf::update_active_config()
{
    if (!is_config_synced_)
    {
        {
            std::lock_guard<std::mutex> lock(config_mutex_);
            active_config_ = ag_config_.config_;
        }
        grp_->tool_id_ = active_config_.tool_frame_id;
        grp_->wobj_id_ = active_config_.user_frame_id;
        grp_->base_frame_id_ = active_config_.base_frame_id;
        is_config_synced_ = true;
    }
    if (is_dyn_related_change_)
    {
        update_dyna_para();
        is_dyn_related_change_ = false;
    }
}

void axisgroup::RobotSetIntf::update_dyna_para()
{
    AXISGROUP_LOG_INFO("Robot %d DYN Changed!", grp_->serial_num_);
    double alpha[MAX_AXIS] = {};
    double a[MAX_AXIS] = {};
    double d[MAX_AXIS] = {};
    double offset[MAX_AXIS] = {};

    double toolParameters[4] = {0};
    double installAngle[3] = {0};

    double kineticParam[MAX_AXIS * 14] = {0};
    double DynamicBsParam[MAX_AXIS * 10 + CART_DIM + CART_DIM] = {0};

    auto payload = RobotPayloadMgr::get_robot_payload(active_config_.work_payload_id).payload();
    toolParameters[0] = payload.m;
    toolParameters[1] = payload.p[0] / 1000.0;
    toolParameters[2] = payload.p[1] / 1000.0;
    toolParameters[3] = payload.p[2] / 1000.0;

    int joint_num = get_joint_num_by_serial_num(serial_num_);
    for (int i = 0; i < joint_num; i++)
    {
        alpha[i] = active_config_.robot_model.dh.robot_dh.alpha.val[i] * PM_PI / 180.0;
        a[i] = active_config_.robot_model.dh.robot_dh.a.val[i] / 1000.0;
        d[i] = active_config_.robot_model.dh.robot_dh.d.val[i] / 1000.0;
        offset[i] = active_config_.robot_model.joint_para.offset.val[i] * PM_PI / 180.0;
    }
    auto base_offset = UsrFrameMgr::get_usr_frame(active_config_.base_frame_id).pose();
    installAngle[0] = base_offset.a / 180.0 * PM_PI;
    installAngle[1] = base_offset.b / 180.0 * PM_PI;
    installAngle[2] = base_offset.c / 180.0 * PM_PI;

    if (joint_num == 6)
    {
        for (int i = 0; i < 60; i++) { kineticParam[i] = active_config_.robot_model.dyn.dyn[i]; }
        for (int i = 0; i < 72; i++) { DynamicBsParam[i] = active_config_.robot_model.dyn.base_dyn[i]; }
    }
    else if (joint_num == 7)
    {
        for (int i = 0; i < 98; i++) { kineticParam[i] = active_config_.robot_model.dyn.dyn[i]; }
        // 七轴机器人采用的DH建模，与动力学库的MDH不一致，需要特殊处理
        alpha[0] = 0;
        alpha[1] = PM_PI / 2;
        alpha[2] = -PM_PI / 2;
        alpha[3] = PM_PI / 2;
        alpha[4] = -PM_PI / 2;
        alpha[5] = PM_PI / 2;
        alpha[6] = -PM_PI / 2;
    }

    grp_->dyna_->DynPara_init_with_full_dh(
        toolParameters, installAngle, kineticParam, kineticParam, a, d, alpha, offset);  // @FIXME liwang 这里需要设置连杆动力学参数
    // 这个函数必须再dynpara_init函数后再调用；  // #TODO
    // if (zucmotConfig->robotModel == MODEL_DU_5010_L || zucmotConfig->robotModel == MODEL_DU_5020_R)
    // {
    //     double rpy[3] = {PM_PI_2, 0.0, PM_PI};  // @FIXME liwang 应该需要从base_offset中解析
    //     set_dyn_base_offset(rpy);
    // }
    // axisgroup::AxisGroupMgr::instance().get_axisgroup(0)->dyna()->DynBsPara_init(a, d, alpha, installAngle, DynamicBsParam);
}

void axisgroup::RobotSetIntf::write_usr_config_to_file()
{
    if (is_config_change_)
    {
        ag_config_.write_usr_config_to_file();
    }
    is_config_change_ = false;
}

void axisgroup::RobotSetIntf::sync_robot_config()
{
    // 关节配置
    for (size_t i = 0; i < grp_->joints.size(); i++)
    {
        grp_->joints[i]->config_.dir = active_config_.robot_model.joint_para.dir[i];
        grp_->joints[i]->config_.joint_zero_offset = active_config_.robot_model.joint_para.offset[i];
        grp_->joints[i]->config_.gear_ratio = active_config_.robot_model.joint_para.gear_radio[i];
        grp_->joints[i]->config_.motor_ratio_torq = active_config_.robot_model.joint_para.motor_tor[i];
    }
    // 运动学配置
    set_axisgroup_dh(active_config_.robot_model.dh.robot_dh, false);

    // 动力学配置
    update_dyna_para();

    // 伺服参数配置
    int lv = 0;
    if (active_config_.robot_safety.collision.collision_detection_enable)
    {
        lv = active_config_.robot_safety.collision.clsn_sensitivity;
    }
    set_collision_lv(0);
}

void axisgroup::RobotSetIntf::set_axisgroup_dh(const RobotDH& robot_dh, bool from_rt)
{
    kine::KineItf::Robot_Config dh;
    dh.robot_dh = robot_dh;
    grp_->kine_->init(dh);
    AXISGROUP_LOG_INFO("Robot %d DH Be set: by %s", grp_->serial_num_, from_rt ? "RT" : "Default");
    AXISGROUP_LOG_INFO("\n%s", robot_dh.get_robot_dh_str().c_str());
}

int axisgroup::RobotSetIntf::get_joint_num_by_serial_num(uint32_t serial_num)
{
    if (get_module_type(serial_num) == kine::KINETYPE_ATOM)
    {
        return 7;
    }
    return 6;
}
int axisgroup::RobotSetIntf::set_robot_joint_limit(std::string tag, int joint_num, double limit)
{
    bool change = true;
    if (tag == "JOINT_POS_MIN")
    {
        ag_config_.config_.joint_limit.min[joint_num] = limit;
        grp_->joints[joint_num]->set_min_pos(limit);
        // #TODO 设置到伺服
    }
    else if (tag == "JOINT_POS_MAX")
    {
        ag_config_.config_.joint_limit.max[joint_num] = limit;
        // #TODO 设置到伺服
        grp_->joints[joint_num]->set_max_pos(limit);
    }
    else if (tag == "JOINT_ERR_LIMIT")
    {
        ag_config_.config_.joint_limit.err_limit[joint_num] = limit;
        // #TODO 设置到伺服
    }
    else if (tag == "JOINT_VEL_LIMIT")
    {
        ag_config_.config_.joint_limit.vel[joint_num] = limit;
        grp_->joints[joint_num]->set_max_vel(limit);
    }
    else
    {
        return -1;
    }
    config_change();
    return 0;
}

int axisgroup::RobotSetIntf::set_usr_frame(int id)
{
    if (id >= 0)
    {
        AXISGROUP_LOG_INFO("Usr Frame id change from %d to %d", ag_config_.config_.user_frame_id, id);
        std::lock_guard<std::mutex> lock(config_mutex_);
        ag_config_.config_.user_frame_id = id;
    }

    // grp_->kine()->fkine(RobJointVal(jointPos), zucmotStatus->carte_pos_cmd, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);

    // if (zucmotStatus->tracking == 1 || zucmotStatus->compliantType != 0)
    // {
    //     double tp_cur_pos[ZUCMOT_MAX_JOINTS] = {};
    //    //     tpManager_tpGetCurJointPos(tp_cur_pos);
    //     //如果是处于传送带跟踪或者柔顺控制状态,则使用规划关节角更新笛卡尔位置
    //     axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //         RobJointVal(tp_cur_pos), zucmotStatus->carte_pos_desired, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
    // }
    // else
    // {
    //     ZucPose finalUserOffset = getFinalUserOffset();
    //     /*keep joint position as current, update the cartesian position*/
    //     axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //         RobJointVal(jointPos), zucmotStatus->carte_pos_desired, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
    // }
    //    // tpManager_tpSetGoalPose(zucmotStatus->carte_pos_desired);
    //    // tpManager_tpSetPose(zucmotStatus->carte_pos_desired);
    // // MOTCMD_INFO("currPos changed: 3 %f, %f, %f\n", zucmotDebug->coord_tp.currentPos.tran.x, zucmotDebug->coord_tp.currentPos.tran.y, zucmotDebug->coord_tp.currentPos.tran.z);
    // double cartPos[CART_DIM] = {0.0};
    // cartPos[0] = zucmotStatus->carte_pos_desired.tran.x;
    // cartPos[1] = zucmotStatus->carte_pos_desired.tran.y;
    // cartPos[2] = zucmotStatus->carte_pos_desired.tran.z;
    // cartPos[3] = zucmotStatus->carte_pos_desired.a * PM_PI / 180.0;
    // cartPos[4] = zucmotStatus->carte_pos_desired.b * PM_PI / 180.0;
    // cartPos[5] = zucmotStatus->carte_pos_desired.c * PM_PI / 180.0;
    // axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->dyna()->admittanceSynpos(cartPos);  // 同步期望位置
    config_change();
    return 0;
}

int axisgroup::RobotSetIntf::set_robot_tool(int id)
{
    if (id >= 0)
    {
        AXISGROUP_LOG_INFO("Tool id change from %d to %d", ag_config_.config_.tool_frame_id, id);
        std::lock_guard<std::mutex> lock(config_mutex_);
        ag_config_.config_.tool_frame_id = id;
    }
    // TP相关问题处理
    // grp_->kine()->fkine(RobJointVal(jointPos), zucmotStatus->carte_pos_cmd, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
    // if (zucmotStatus->tracking == 1 || zucmotStatus->compliantType != 0)
    // {
    //     double tp_cur_pos[ZUCMOT_MAX_JOINTS] = {};
    //     //     tpManager_tpGetCurJointPos(tp_cur_pos);
    //     //如果是处于传送带跟踪或者柔顺控制状态,则使用规划关节角更新笛卡尔位置
    //     axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //         RobJointVal(tp_cur_pos), zucmotStatus->carte_pos_desired, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);
    // }
    // else
    // {
    //     ZucPose finalUserOffset = getFinalUserOffset();
    //     axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //         RobJointVal(jointPos), zucmotStatus->carte_pos_desired, &zucmotConfig->tool_offset, &finalUserOffset, &zucmotConfig->base_offset);
    // }
    // /*keep joint position as current, update the cartesian position*/
    // tpManager_tpSetGoalPose(zucmotStatus->carte_pos_desired);
    // tpManager_tpSetPose(zucmotStatus->carte_pos_desired);

    // axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->dyna()->admittanceSynpos(cartPos);  // 同步期望位置
    config_change();
    return 0;
}

int axisgroup::RobotSetIntf::set_collision_lv(int lv)
{
    for (size_t i = 0; i < grp_->joint_num(); i++) { grp_->joints[i]->sync_collision_lv(lv); }
    return 0;
}

void axisgroup::RobotSetIntf::set_payload(int id)
{
    // -1 表示整个payload列表发生了变化
    if (id != -1)
    {
        AXISGROUP_LOG_INFO("Payload id change from %d to %d", ag_config_.config_.work_payload_id, id);
        std::lock_guard<std::mutex> lock(config_mutex_);
        ag_config_.config_.work_payload_id = id;
    }
    config_change();
    is_dyn_related_change_ = true;
}

void axisgroup::RobotSetIntf::set_base_offset()
{
    AXISGROUP_LOG_INFO("Base offset change %d : %s", grp_->base_frame_id_, axisgroup::UsrFrameMgr::get_usr_frame(grp_->base_frame_id_).pose_str().c_str());
    is_dyn_related_change_ = true;
}

int axisgroup::RobotSetIntf::get_serial_type(uint32_t serial_num) { return serial_num / 10000; }
int axisgroup::RobotSetIntf::get_module_type(uint32_t serial_num)
{
    int robtype = serial_num / 100000000;
    switch (robtype)
    {
    case kine::KINETYPE_ZU:
    case kine::KINETYPE_MINI:
    case kine::KINETYPE_ATOM:
    case kine::KINETYPE_MAX:
    case kine::KINETYPE_A:
        return robtype;
    default:
        return -1;
    }
}
